/*-
 * Copyright (c) 2014 Shang-Rong Cai. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE AUTHORS AND CONTRIBUTORS ``AS IS'' AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED.  IN NO EVENT SHALL AUTHOR OR CONTRIBUTORS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
 * SUCH DAMAGE.
 *
 * $ sim.c    Rev. 0.1.0    2014-05-28T15:57+08:00 $
 */

#include <stdio.h>
#include <math.h>

#include <itermth.h>


int vector_set_zeros(size_t length, double *vec)
{
    int ierr = 0;

    size_t zi;

    for (zi = 0; zi < length; zi++)
    {
        vec[zi] = 0;
    }

    return ierr;
}


int vector_stdout(size_t length, double *vec)
{
    int ierr = 0;

    size_t zi;

    for (zi = 0; zi < length; zi++)
    {
        printf("%4zd: % .14le\n", zi, vec[zi]);
    }

    return ierr;
}


int vector_copy(size_t length, const double *vec_x, double *vec_y)
{
    int ierr = 0;

    size_t zi;

    for (zi = 0; zi < length; zi++)
    {
        vec_y[zi] = vec_x[zi];
    }

    return ierr;
}


/* Compute Vector (Array) Difference of Components: vec_vz = vec_vx - vec_vy */
int vector_diff(const size_t length, const double *vec_vx, const double *vec_vy, double *vec_vz)
{
    int ierr = 0;

    size_t zi;

    for (zi = 0; zi < length; zi++)
    {
        vec_vz[zi] = vec_vx[zi] - vec_vy[zi];
    }

    return ierr;
}


double vector_dot_vector(const size_t length, const double *vec_vx, const double *vec_vy)
{
    double dot_value;
    size_t zi;

    dot_value = 0;

    for (zi = 0; zi < length; zi++)
    {
        dot_value += vec_vx[zi] * vec_vy[zi];
    }

    return dot_value;
}


double vector_norm2(const size_t length, const double *vec_vx)
{
    double nrm2_val;

    size_t zi;

    nrm2_val = 0;

    for (zi = 0; zi < length; zi++)
    {
        nrm2_val += vec_vx[zi] * vec_vx[zi];
    }

    nrm2_val = sqrt(nrm2_val);

    return nrm2_val;
}


int vector_scale_copy(const size_t length, const double *vec_x, const double cof, double *vec_y)
{
    int ierr = 0;

    size_t zi;

    for (zi = 0; zi < length; zi++)
    {
        vec_y[zi] = vec_x[zi] * cof;
    }

    return ierr;
}


/* Stationary Iterative Method */

/* (Weighted) Jacobi Iteration */
/* Jacobi Method Solve Ax=b */
int jacobi_cidk(const cidk_t *cidk, const double *vrhs, const int max_iter, const double atol, const double rtol, const double omega, double *vsol, int *iter)
{
    int ierr = 0;
    size_t zi;
    sidx_t si;

    double omega_bar;  /* 1.0 - omega */
    int cur_iter;  /* Current iteration */

    double *vec_x0, *vec_x1;
    double *vec_swap;

    double *vec_res;  /* Residual vector */
    double *buf_vec;  /* Buffer for vector (compute Ax) */

    double val_diag;

    double nrm2_r0, nrm2_rk;  /* 2-norm of residual vector */
    double nrm_df;
    double nrm_v1;

    /* Allocate Memory For Vectors */
    vec_x0 = (double*) calloc(cidk->r_sz, sizeof(double));
    vec_x1 = (double*) calloc(cidk->r_sz, sizeof(double));

    vec_res = (double*) calloc(cidk->r_sz, sizeof(double));
    buf_vec = (double*) calloc(cidk->r_sz, sizeof(double));

    /* Copy Initial Vector */
    for (zi = 0; zi < cidk->r_sz; zi++)
    {
        vec_x0[zi] = vsol[zi];
    }

    omega_bar = 1.0 - omega;
    *iter = 0;  /* Initail iteration counter */

    /* Jacobi Iteration */
    if (cidk->flag == ROW_WISE)
    {
        /* Compute Residual 2 Norm */
        ierr = cidk_vector_multi(cidk, vec_x0, buf_vec);
        ierr = vector_diff(cidk->r_sz, vrhs, buf_vec, vec_res);

        nrm2_r0 = vector_norm2(cidk->r_sz, vec_res);

        /* Main Iteration */
        for (cur_iter = 0; cur_iter < max_iter; cur_iter++)
        {
            /* Compute Residual 2 Norm */
            ierr = cidk_vector_multi(cidk, vec_x0, buf_vec);
            ierr = vector_diff(cidk->r_sz, vrhs, buf_vec, vec_res);

            nrm2_rk = vector_norm2(cidk->r_sz, vec_res);

            printf("%4d Residual 2-Norm: %.7le\n", cur_iter, nrm2_rk);

            /* Stoping Critial */
            if (nrm2_rk / nrm2_r0 < rtol || nrm2_rk < atol)
            {
                break;
            }

            for (zi = 0; zi < cidk->r_sz; zi++)
            {
                vec_x1[zi] = vrhs[zi];

                for (si = cidk->sptr[zi].head; si < cidk->sptr[zi].tail; si++)
                {
                    //printf("(%6zd, %6d) % .14le\n", zi, cidk->item[si].index, cidk->item[si].value);

                    if (zi != (size_t) cidk->item[si].index)
                    {
                        vec_x1[zi] -= cidk->item[si].value * vec_x0[cidk->item[si].index];
                    }
                    else
                    {
                        val_diag = cidk->item[si].value;
                    }
                }

                vec_x1[zi] = omega * vec_x1[zi] / val_diag + omega_bar * vec_x0[zi];
            }

            /* Swap Vectors vec_x0, vec_x1 */
            vec_swap = vec_x0;
            vec_x0 = vec_x1;
            vec_x1 = vec_swap;

            (*iter)++;
        }
    }
    else
    {
        printf("NO SUPPORT NOW\n");
    }

    /* Copy Solution Vector */
    for (zi = 0; zi < cidk->r_sz; zi++)
    {
        vsol[zi] = vec_x1[zi];
    }

    /* Free Memory */
    free(vec_x0);
    free(vec_x1);

    free(vec_res);
    free(buf_vec);

    return ierr;
}


/* Gauss Seidel Iteration */
/* Successive Over-Relaxation Iteration */
int sor_cidk(const cidk_t *cidk, const double *vrhs, const int max_iter, const double atol, const double rtol, const double omega, double *vsol, int *iter)
{
    int ierr = 0;
    size_t zi;
    sidx_t si;

    double omega_bar;  /* 1.0 - omega */
    int cur_iter;  /* Current iteration */

    double *vec_x0, *vec_x1;
    double *vec_swap;

    double *vec_res;  /* Residual vector */
    double *buf_vec;  /* Buffer for vector (compute Ax) */

    double val_diag;

    double nrm2_r0, nrm2_rk;  /* 2-norm of residual vector */
    double nrm_df;
    double nrm_v1;

    /* Allocate Memory For Vectors */
    vec_x0 = (double*) calloc(cidk->r_sz, sizeof(double));
    vec_x1 = (double*) calloc(cidk->r_sz, sizeof(double));

    vec_res = (double*) calloc(cidk->r_sz, sizeof(double));
    buf_vec = (double*) calloc(cidk->r_sz, sizeof(double));

    /* Copy Initial Vector */
    for (zi = 0; zi < cidk->r_sz; zi++)
    {
        vec_x0[zi] = vsol[zi];
    }

    omega_bar = 1.0 - omega;
    *iter = 0;  /* Initail iteration counter */

    /* Jacobi Iteration */
    if (cidk->flag == ROW_WISE)
    {
        /* Compute Residual 2 Norm */
        ierr = cidk_vector_multi(cidk, vec_x0, buf_vec);
        ierr = vector_diff(cidk->r_sz, vrhs, buf_vec, vec_res);

        nrm2_r0 = vector_norm2(cidk->r_sz, vec_res);

        /* Main Iteration */
        for (cur_iter = 0; cur_iter < max_iter; cur_iter++)
        {
            /* Compute Residual 2 Norm */
            ierr = cidk_vector_multi(cidk, vec_x0, buf_vec);
            ierr = vector_diff(cidk->r_sz, vrhs, buf_vec, vec_res);

            nrm2_rk = vector_norm2(cidk->r_sz, vec_res);

            printf("%4d Residual 2-Norm: %.7le\n", cur_iter, nrm2_rk);

            /* Stoping Critial */
            if (nrm2_rk / nrm2_r0 < rtol || nrm2_rk < atol)
            {
                break;
            }

            for (zi = 0; zi < cidk->r_sz; zi++)
            {
                vec_x1[zi] = vrhs[zi];

                for (si = cidk->sptr[zi].head; si < cidk->sptr[zi].tail; si++)
                {
                    //printf("(%6zd, %6d) % .14le\n", zi, cidk->item[si].index, cidk->item[si].value);

                    if (zi < (size_t) cidk->item[si].index)
                    {
                        vec_x1[zi] -= cidk->item[si].value * vec_x0[cidk->item[si].index];
                    }
                    else if (zi > (size_t) cidk->item[si].index)
                    {
                        vec_x1[zi] -= cidk->item[si].value * vec_x1[cidk->item[si].index];
                    }
                    else
                    {
                        val_diag = cidk->item[si].value;
                    }
                }

                vec_x1[zi] = omega_bar * vec_x0[zi] + omega * vec_x1[zi] / val_diag;
            }

            /* Swap Vectors vec_x0, vec_x1 */
            vec_swap = vec_x0;
            vec_x0 = vec_x1;
            vec_x1 = vec_swap;

            (*iter)++;
        }
    }
    else
    {
        printf("NO SUPPORT NOW\n");
    }

    /* Copy Solution Vector */
    for (zi = 0; zi < cidk->r_sz; zi++)
    {
        vsol[zi] = vec_x1[zi];
    }

    /* Free Memory */
    free(vec_x0);
    free(vec_x1);

    free(vec_res);
    free(buf_vec);

    return ierr;
}
